#include "../../../XMLTools.h"
#include "../../../MathTools.h"

#include "Force3DSensorMeasurement.h"

using namespace MMM;

Force3DSensorMeasurement::Force3DSensorMeasurement(float timestep, const Eigen::Vector3f &force, SensorMeasurementType type) :
    InterpolatableSensorMeasurement(timestep, type),
    force(force)
{
}

SensorMeasurementPtr Force3DSensorMeasurement::clone() {
    return clone(timestep);
}

bool Force3DSensorMeasurement::equals(SensorMeasurementPtr sensorMeasurement) {
    Force3DSensorMeasurementPtr ptr = boost::dynamic_pointer_cast<Force3DSensorMeasurement>(sensorMeasurement);
    if (ptr) {
        return force == ptr->force;
    }
    return false;
}

Force3DSensorMeasurementPtr Force3DSensorMeasurement::clone(float newTimestep) {
    Force3DSensorMeasurementPtr clonedSensorMeasurement(new Force3DSensorMeasurement(newTimestep, force, type));
    return clonedSensorMeasurement;
}

void Force3DSensorMeasurement::appendMeasurementDataXML(RapidXMLWriterNodePtr measurementNode) {
    measurementNode->append_node("Force")->append_data_node(XML::toString(force));
}

Force3DSensorMeasurementPtr Force3DSensorMeasurement::interpolate(Force3DSensorMeasurementPtr other, float timestep, SensorMeasurementType type) {
    Eigen::Vector3f interpolatedForce = Math::linearInterpolation(this->force, this->timestep, other->force, other->timestep, timestep);
    Force3DSensorMeasurementPtr interpolatedSensorMeasurement(new Force3DSensorMeasurement(timestep, interpolatedForce, type));
    return interpolatedSensorMeasurement;
}

Eigen::Vector3f Force3DSensorMeasurement::getForce() {
    return force;
}
